import lejos.nxt.*;
import lejos.robotics.navigation.*;

public class NavigationTester {

	private static final float DIAMETER = 5.6F; // cm
	private static final float TRACKWIDTH = 17.8F; // cm
	private static final Motor LEFTMOTOR = Motor.B;
	private static final Motor RIGHTMOTOR = Motor.C;
	private static final boolean REVERSE = true;
	
	public static void main(String[] args) {
		TachoPilot pilot = new TachoPilot(DIAMETER, TRACKWIDTH, LEFTMOTOR, RIGHTMOTOR, REVERSE);
		SimpleNavigator robot = new SimpleNavigator(pilot);
		robot.goTo(20, 0); // 20 cm straight forward
		robot.rotateTo(90);
		robot.travel(20); // 20 cm to the side
		//robot.updatePose();
		//robot.updatePosition();
		robot.goTo(20, 0); // In which direction do we now travel?
	}

}
